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Abstract — We begin this paper by presenting our approach 
to robot manipulation, which emphasizes the benefits of making 
contact with the world across the entire manipulator. We assume 
that low contact forces are benign, and focus on the development 
of robots that can control their contact forces during goal- 
directed motion. Inspired by biology, we assume that the robot 
has low-stiffness actuation at its joints, and tactile sensing across 
the entire surface of its manipulator. We then describe a novel 
controller that exploits these assumptions. The controller only 
requires haptic sensing and does not need an explicit model of 
the environment prior to contact. It also handles multiple contacts 
across the surface of the manipulator. The controller uses model 
predictive control (MPC) with a time horizon of length one, and 
a linear quasi-static mechanical model that it constructs at each 
time step. We show that this controller enables both real and 
simulated robots to reach goal locations in high clutter with 
low contact forces. Our experiments include tests using a real 
robot with a novel tactile sensor array on its forearm reaching 
into simulated foliage and a cinder block. In our experiments, 
robots made contact across their entire arms while pushing aside 
movable objects, deforming compliant objects, and perceiving the 
world. 



I. Introduction 

Research on robot manipulation has often emphasized col- 
lision free motion with occasional contact restricted to the 
robot's end effector. In essence, most of the manipulator's 
motion is intended to be free- space motion and unintended 
contact is considered to be a failure of the system. In contrast, 
animals often appear to treat contact between their arms and 
the world as a benign and even beneficial event that does 
not need to be avoided. For example, humans make extensive 
contact with their forearms even during mundane tasks, such 
as eating or working at a desk. 

Within this paper, we present progress towards new founda- 
tional capabilities for robot manipulation that take advantage 
of contact across the entire arm. Our primary assumption is 
that, for a given robot and environment, contact forces below 
some value have no associated penalty. For example, when 
reaching into a bush, moderate contact forces are unlikely to 
alter the robot's arm or the bush in undesirable ways. Likewise, 
even environments with fragile objects, such as glassware on 
a shelf, can permit low contact forces. While some situations 
merit strict avoidance of contact with an object, we consider 
these to be rare, and instead focus on default strategies that 
allow contact. 

In order to keep contact forces low enough to avoid penal- 
ties, we further assume that the robot arm has compliant 
actuation at its joints and tactile sensing across all of its 
surfaces. Low-stiffness compliant actuation can reduce contact 
forces due to perturbations, error, and other sources. Tactile 
sensing enables direct monitoring of contact forces (and the 
distribution of contact forces). These assumed hardware capa- 
bilities for the robot's actuation and sensing are also analogous 
to capabilities found in animals. 




Fig. 1: Left: View of foliage from the robot's perspective. 
Two rigid blocks of wood are occluded by the leaves. Right: 
Image of the robot after it has successfully reached the goal 
location using the controller we present in this paper. The red 
circle denotes the position of the end effector 



Our main contribution in this paper is a novel controller 
that enables a robot arm to move within an environment 
while regulating contact forces across its entire surface. The 
controller uses model predictive control (MPC) with a time 
horizon of length one and a linear quasi- static mechanical 
model. At each time step, the controller constructs a model 
and solves an associated quadratic programming problem in 
order to minimize the predicted distance to a goal subject to 
constraints on the predicted contact forces. 

We also empirically evaluate our controller's performance 
with respect to the task of haptically reaching to a goal location 
in high clutter (see Fig. [T]). We assume that the clutter can 
consist of a variety of fixed, movable, and deformable objects, 
and that the robot does not have a model of the environment in 
advance. This task is representative of real- world challenges 
for robots, such as retrieving objects from rubble, foliage, or 
the back of a shelf. It is also representative of an animal 
reaching for food while foraging. 

We tested our controller under a variety of conditions with 
a simulated robot, a real robot with simulated tactile sensing, 
and a real robot with real tactile sensors across its forearm. 
For many of the tasks, the robots compressed, bent, or moved 
objects out of the way with their arms while reaching the goal 
location. Our results demonstrate that the model predictive 
controller has a higher success rate and lower contact forces 
compared to a baseline controller. 

A. Biological Inspiration 

Animals serve as an inspiration for our research (see Fig. [2]), 
especially in terms of the capabilities they exhibit, their 
sensing, and their actuation. 

Animals dramatically outperform current autonomous 
robots within unstructured environments, such as when for- 
aging in dense foliage. During these activities, animals often 
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Fig. 2: We propose foundational capabilities for robotic manipulation that will enable robots to make and exploit contact with 
their environment. While foraging for food, animals and humans make contact at multiple locations on their arm and operate 
in cluttered environments, (a) A raccoon reaches into a bird house to find eggs and young \webpage\ \2011a\ . (b) A Long-tailed 
Macaque grasps fruit in dense foliage (webpagej \2011b ). (c) When noodling, people find catfish holes from which to pull fish 
out {webpage 2011c ). (d)-(e) A person makes contact along his forearm while reaching for an object in the back of a shelf 
and refrigerator (All images used with permission) 



make contact with the world at multiple locations along their 
arms, and reach into visually occluded spaces. 

Touch is an important sensory modality for successful forag- 
ing ( Dominyl |20Q4[ |Iwaniuk and Whishaw[ |1999| ). In general, 
animals can usefully manipulate the world in the absence of 
vision. As an extreme example, the star-nosed mole uses the 
sense of touch almost exclusively while foraging ( [Catania 
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1999| ). Humans also competently manipulate the world without 
vision, as the reader can demonstrate by haptically exploring 
the underside of a nearby table. Inspired by these capabilities, 
our goal is to develop methods that degrade gracefully when 
deprived of non-haptic modalities, such as vision and audition. 

Animals also serve as inspiration for our decision to assume 
the presence of whole-body tactile sensing. Although whole- 
body tactile sensing is currently rare in robotics, it is nearly 
ubiquitous in biology, which suggests that it is advantageous 
for operation in unstructured environments. Organisms from 
small nematodes to insects and mammals are able to sense 
forces across their entire bodies ( |Bianchi| |2007t [Goodman! 
20061 jLederman and Klatzkyj [20091 jLumpkin et"all [2QTQ| ). 
Sensing forces also plays an important role in avoiding injury. 
For example, loss of sensitivity in a human diabetic's foot is 



a strong risk factor for injuring the foot ( [Sims Jr et al| [1988] ). 
As has often been noted, tactile sensing also supports human 



manipulation (Johansson and Flanagan 2009). 



Compliant actuation at the joints is another common char- 
acteristic in animals that we have chosen to emulate ( |Alexan-| 
derj p^90l jHoganj p^84l jMigliore et"aTl [2005] ). Robotics 
researchers have demonstrated that compliant joints lower 
interaction forces during incidental contact and can be ben- 
eficial for unmodeled and dynamic interactions (jBuerg er and 
Hoganj [20071 |J- P^att and Pratt[ [2QQT1 [Pratt| r2002 ; Pra tt and 
Williamson 1995). This capability is now relatively common 
within robotics, although we use stiffnesses that tend to be 
lower than other published research. For example, in some 
postures, the stiffness at the end effector is a factor of five 



lower than those reported by DLR in Ott et al (2007). The 
values we use are similar to measured stiffnesses of humans 
during planar reaching motions fShadmehr 1993[ ). 
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Fig. 3: Example illustrating the available range of motion for 
a 1 DoF arm if the controller uses a safety margin with non- 
contact sensing (left), allows contact with an object (middle), 
and allows the arm to push into compliant and movable objects 
(right). 



B. Benefits of Whole-body Contact and Tactile Sensing 

Given our emphasis on whole-body contact and whole-body 
tactile sensing, we now illustrate some of the performance 
benefits associated with these design decisions. 

One benefit of allowing contact with the arm is the increased 
effective range of motion of the manipulator. As illustrated 
in Fig. [3j the performance loss due to avoiding contact is 
exacerbated by safety margins and an inability to apply forces 
that compress or move objects. Similarly, if the robot has 
a compliant exterior, avoiding contact forfeits the additional 
range of motion achievable by compressing this exterior. 
Animals appear to use this method to achieve greater motion 
while in contact and squeeze through openings. The reader can 
gain some insight into this by noting the compressibility of the 
human forearm that results from the soft tissues surrounding 
the endoskeleton. 

Whole-body tactile sensing with high spatial resolution also 
has advantages in terms of distinguishing between distinct 
contact configurations and force distributions, and measuring 
forces with high sensitivity. Prior research has attempted to 
use the geometry of links, measurements of joint torques and 
force-torque sensors to estimate contact properties (e.g, Bicchi 
'eraI1fT99?);'De Schutter et al| |l999l ); [Eberman and Salisbury 
(1990); Kaneko and Tanie ( 1994] )). However, interpretation of 
data from these sensors can often be ambiguous in multi- 
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Fig. 4: 5'6>m^ multi-contact conditions can not be detected 
(left) or distinguished (right) using only joint torque sensing 
or force-torque sensors mounted at the joints. We can detect 
and distinguish between these conditions using tactile sensors 
covering the arm. 



contact situations ("Salisbury', '1984"). In practice, the estimation 
can also be sensitive to the configuration of the manipulator, 
the fidelity of the torque estimates, and friction and flexibility 
at the joints f Pogar et al] |201Qt |Eberman| |1989| ). 

Fig. [4] shows two examples of contact conditions that will 
result in ambiguity if a robot only uses joint torque sensing or 
force-torque sensors mounted at the joints. In the first example, 
the resultant force and torque on the robot arm is zero, but it is 
wedged between two contacts. The second example illustrates 
that contact over a large area, and a high force at a single 
point can result in the same total resultant force and torque. 

Distinguishing among these situations can be advantageous. 
For example, a high total force distributed over a small area, 
such as due to contact with the edge of a cinder block or 
a small branch, has greater potential to damage the robot or 
the world, respectively. Similarly, the same large total force 
distributed across a large area due to contact with tall grass 
or leaves is less likely to damage the robot or the world. 
Moreover, the geometries associated with distinct contact 
regions, such as a rigid point, line, or plane, imply distinct 
options for subsequent movement. 

More generally, for many manipulation tasks, the manipu- 
lator primarily influences the world via contact forces with 
other forms of physical interaction, such as heat transfer, 
being uncontrolled or irrelevant. As such, we expect that 
direct measurement of contact forces will enable superior 
manipulation capabilities. 

C. Challenges Associated with Reaching in High Clutter 

For this paper, we focus on the task of reaching to a goal 
location in high clutter. This entails a number of challenges, 
including the following: 

• Lack of non-contact trajectories: As clutter increases, 
approaches that avoid contact with the environment will 
have a diminishing set of trajectories that can successfully 
perform the task. If the robot is interacting with movable 
or compliant objects, such as foliage, reaching the goal 
while applying low forces might be possible but non- 
contact trajectories may not exist. 

• Contact with only the end effector may be inefficient or in- 
feasible: Removing or rearranging the clutter by making 
serial contact with only the end effector may be inefficient 
or infeasible. For example, given an environment with 
multiple compliant objects (e.g., plants) it may not be 
possible to first bend each of the objects out of the way 
one at a time without plastic deformation. Instead, an 



efficient solution would be to directly reach to the goal 
and allow multiple contacts to occur with the compliant 
objects and the arm. 

• Clutter can consist of unique objects and configurations 
that have not been encountered before: For some types 
of natural clutter, such as dense foliage, each object 
encountered can be unique. Objects could be fixed, mov- 
able, rigid, deformable, granular, fluid-like, and dynamic. 
Likewise, the configuration of the environment can be 
unique. Statistical properties may be informative, but a 
specific environment may only be encountered once by 
the robot. 

• Observation of geometry is obstructed: Low visibility due 
to occlusion will often prevent conventional line of sight 
sensors, such as cameras and laser range finders, from 
modeling the geometry of the clutter in advance (see 

Fig.Q. 

• Mechanics are difficult to infer without contact: Non- 
contact sensing provides limited ability to infer the me- 
chanical properties of the clutter, such as whether or not 
an object can be bent or moved out of the way. Likewise, 
objects may be mechanically coupled in complicated 
ways, such as through adhesion or unobserved rigid 
connections. 

Notably, many approaches to manipulation are poorly 
matched to address these challenges. For example, approaches 
that rely on preexisting detailed models, estimation of mod- 
els via conventional line-of-sight sensing, or collision-free 
motions with the arm would fare poorly under real-world 



conditions at which animals excel (e.g., Kavraki and LaValle 
([2008]); [Saxena et al| ( [2008] ); [Srinivasa et al| ( |2009| ); [Stilman 
|eFaIl ( |2QQ7] )). 

D. Our Approach 

In contrast, our approach directly addresses these challenges 
associated with high clutter due to the following properties: 

• We explicitly allow multiple contacts across the entire 
surface of the arm: Our approach assumes that the 
entire surface of the manipulator is covered with pressure 
sensing elements (tactile pixels or taxels), and that every 
taxel could be simultaneously in contact with the world 
at any given moment. 

• We do not require a detailed model of the environment 
prior to contact: Our approach only requires that initial 
parameters appropriate for the robot, the environment, 
and the task be provided to the robot's control system 
in advance. For our current controller, this includes the 
force magnitude below which no penalty is expected, and 
the initial stiffness estimate assigned to new contacts. 

• We only require contact-based sensing: Our approach 
only requires contact-based sensing. Our current con- 
troller only makes use of haptic sensing in the form of 
joint angleqj and taxel responses. Through this contact- 
based sensing and a kinematic model of its own arm, the 
controller estimates the instantaneous contact geometry 

^Due to the low-stiffness virtual visco-elastic springs at the robot's joints, 
the joint angles over time directly relate to the joint torques. 



and the stiffness associated with each contact in order 
to generate a local model. Future work may make use of 
non-contact sensing, which would be complementary, but 
this is not required. 

E. Organization of this Paper 

The rest of this paper is organized as follows. In Sec. |ll| 
we discuss related research on manipulation in clutter, multi- 
contact manipulation, motion planning with deformable ob- 
jects, robot locomotion, and model predictive control. Next, 



during an off-line modeling stage). Additionally, for now, the 
planned actions are executed without sensor feedback. 

Mason et al ( 2Q11| ) describes a simple end effector design 



in Sec. Ill we derive our model predictive controller for the 
task of reaching to a goal location in a cluttered environment. 
We describe the hierarchy of controllers running on our robot, 
the linear quasi- static model that the controller uses, and the 
quadratic program that the controller solves at each time step. 
We then describe three testbeds that we used to empirically 
test the performance of our controller (Sec. IIV]) and the 



baseline controller that we compared our model predictive 
controller against (Sec.|V]). Next, we describe the experiments 
that we ran in Sec. [Vll We end with a discussion of our work 
in Sec. |VII| a nd conclude with a brief summary of our results 
in Sec. lVlTll 

II. Related Work 
A. Manipulation in Clutter 

Within this paper, our goal is to enable robots to reach to 
a goal location in cluttered environments and manipulate with 
multiple contacts across the entire arm using haptic sensing. 

In contrast, robotics research has often addressed the task of 
generating collision free trajectories (e.gJKavraki and LaValle] 
poos] ); ILaValle and Kuffner] ( |200T] ); [Lozano-Perez ( 1987j ), 



generating reaching motions in free space (e.g, [Hersch and 
Billardl ([2006]); |Metta et al| ( |2QTT] ); |Stulp et al| ( |2009| )), and 



manipulating objects in uncluttered environments (e.g, |Hsiao| 
[^ al (2010); Jain and Kemp (2010a); Natale and Torres- Jara 
T2OO61 ); [Pastor et al, ( ,201 1, ); [Romano et al| ( |2011| ); |Saxena et al, 
2008])). 



Research has also looked at the problem of manipulation 
in cluttered environments. However, most prior research on 
manipulation in clutter with autonomous control and during 
teleoperation (e.g, Leeper et al ( 2012| )) restricts contact be- 
tween the robot and its environment to the end effector. Often, 
prior research has also used non-contact, line of sight sensors 
and required pre-existing models of objects. 

[Stilman et al| ( |2007| ) describes an algorithm for planning 
in an environment with movable obstacles. Within software 
simulation, the planner uses geometric models of all the 
objects in the world to enable a robot to rearrange clutter by 
grasping and moving objects, and opening doors. 

Dogar and Srinivasa| p011| ) presents a framework to plan 



that can be used to grasp a single marker from a cluttered 
pile of nearly identical markers, and haptically estimate the 
marker's pose after grasping it. This is the most similar work 
in spirit to ours, since it investigates manipulation in high 
clutter, does not use a detailed model of the environment 
prior to contact, allows multiple contacts across the surface 
of the end effector, and uses haptic sensing. Their "Let the 
fingers fall where they may." approach to grasping in clutter 
has other notable similarities to our approach to reaching 
in clutter. Both approaches use greedy controllers that are 
run iteratively, and both approaches ignore the details of 
how the clutter responds to the robot's actions. However, 
our approach performs more complex feedback-based control 
of the manipulator and does not use a simple mechanism 
nor simple sensing, which they emphasize. This is in part 
because we wish to regulate the contact forces. More degrees 
of freedom also appear to improve the performance of our 
controller, although we do not yet have reportable results to 
support this claim. In addition, we focus on reaching in clutter, 
rather than grasping, and present empirical results for diverse 
environments, in contrast to their experiments with a collection 
of durable, rigid, nearly-identical, manufactured objects. 

B. Multi-contact Manipulation 

Park and Khatib| ( [2"008| ) presents a framework for controlling 



a robot with multiple contacts along the links. It generalizes 
previous direct force control methods (KhatiBj |1987t Raibert 
and Craig"! |1981| ) to not require force and motion to be along 



a sequence of actions such as pushing and grasping objects 
to rearrange clutter prior to grasping an object. The actions 
currently used within the framework restrict contact to the 
robot's end effector and avoid other contact with the world. 
The implementation relies on estimating the pose of objects in 
the environment using visual and geometric models (created 



orthogonal directions in Cartesian space and to allow for 
contacts at points other than the end effector. 

This method requires a full dynamic model of the robot and 
assumes stationary and rigid contacts. Further, this framework 
assumes that the robot has at least six degrees of freedom 
(DoF) for each contact, to control the contact force and torque 
vector ( [Sentis et"al| 2010| ). A seven degree of freedom arm, like 
the robot arm that we use, with multiple contacts is unlikely 
to have six independent degrees of freedom for each contact. 

Using this framework, results have been shown in simula- 
tion dSentis and Khatib| [20051 |Sentis et al] [2010]), and on a 
real robot in relatively controlled settings (" Petrovskaya et al| 
I 2007| ). No results have been shown in cases where the robot 
makes additional unpredicted contact with the environment or 
loses contact at some locations. 

In contrast, our controller uses a linear quasi- static model 
of the robot's interaction with the environment and does not 
assume that the robot has six degrees of freedom for each con- 
tact. On a real robot with a tactile skin sensor, we demonstrate 
that our controller can operate in cluttered environments with 
multiple unpredicted contacts with compliant, rigid, movable, 
and fixed obstacles across the entire arm of the robot. 

Research in motion planning for humanoid robots has 
shown that a robot with geometric models of its environment 
can make contact at multiple, predetermined locations on its 
body to better perform a task, such as lean on a table to take a 



large foot step ( [Legagne et al 2011 ), use contacts at hands and 
knees to climb a ladder in simulation ("Hauser et al||2QQ5| , and 
use contact between the hand and a table while sitting down 
( Escande and Kheddar 2QQ9| ). These approaches require a 
complete geometric model of the world (which can be difficult 
or impossible to obtain unless the robot is operating in a 
controlled environment), assume stationary and rigid contacts, 
and do not incorporate sensor feedback as the robot (real or 
simulated) executes the planned kinematic trajectory. These 
approaches attempt to maintain balance on the humanoid robot 
while we assume that the robot is statically stable. 

There is also research on multi-contact manipulation within 
the context of using all the surfaces of a multi-fingered hand, 
or the entire body to grasp and manipulate a single object (e.g, 
Bicchi| ( |1993| ); [Bicchi and Kumar (2000 j ; Hsiao and Lozano^ 
Perezl ( |2QQ6l ); |Platt Jr et al| ( [2003] )). 



C Motion Planning with Deformable Objects 

Manipulation research often assumes that objects that the 
robot interacts with are rigid. At the same time, there is 
research on motion planners that allow the robot to make 



contact with, and push into deformable objects (e.g. Frank 
eTall ( [20TT] ); |Patil et al| (|20TTJ; [Rodriguez et~all ( |2006| )). 
However, these approaches assume knowledge of the specific 
configuration of the objects and require accurate and detailed 
models of how objects deform. We avoid these assumptions 
in our work. 

These approaches build object deformation models by using 
data-driven methods for a specific object, or computationally 
expensive physics simulations that use the physical properties 
of the objects. Accurate object deformation models can be hard 
to obtain in realistic and cluttered environments. Additionally, 
if multiple objects are in contact with each other and the 
specific configuration is unknown, then building these models 
before making contact may not be feasible. 

D. Robot Locomotion 

Our approach to robot manipulation has similarities to 
approaches that have been successful for robot locomotion. 
For example, researchers have developed robots that locomote 
in cluttered environments without detailed geometric models 
of the terrain nor planning over long time horizons ( [Raibert 



et aH|2008{|Saranli et al||2001| ). Likewise, whole body contact, 
and contact in general, has not been considered undesirable. 
For example, robots have used contact all over their bodies 



to traverse the ground and swim in granular media (Maladen 



et al| |2010[ [McKenna etal] |2008| ). Additionally, the use of 
simple mechanical models, compliance, and force sensing is 
common for robot locomotion ( [Garcia et~all |1998t [J. Pratt and[ 



[Prattl [200T| [Prattl [20021 [Raibert et al| [2008 



). 



E. Model Predictive Control 

One of the initial application areas for model predictive 



control (MPC) was chemical process control (Garcia et al 



1989). It is often referred to as receding horizon control 
when used for control of aerial vehicles ( [Abbeel et al[ [2010| 



Bellingham et al| 2002). MPC has also been used in research 
in robot locomotion (e.g, 'Erez et al ( 2011 ); Manchester et al 
( 20 1 IJ ; Wieber_ (2006 J) , and for controlling robot manipulators 
(e.g, [From et ai[poil| ; [Ivaldi et al[ ( [2QTo1 ); [Kulchenko "md 
|Todorov[ ( [20lT] )). 



III. Model Predictive Controller 

The controller that we have developed uses linear model 
predictive control (MPC) with a time horizon of length one. 
Specifically, using the notation of Morari and Lee ( 1999] ), our 
controller uses a linear discrete time model of the system, 



x{k + 1) = Ax{k) + Bu{k), 



(1) 



where x{k) is the state of the system and u{k) is the control 
input. 

At each time step, k, the controller computes a sequence 
of control inputs, u*{i)^i = k . . .{k ^ N — 1), to minimize 
a quadratic objective function of x(/c),...,x(/c + N) and 
u{k)^ . . . ^u{k-\-N — l), subject to linear inequality constraints 
on x(/c), . . . , x{k + N) and u{k)^ . . . , u{k -\- N — 1), where 
N is the length of the time horizon of the model predictive 



controller. This defines a quadratic program ( [Morari and Lee| 
[1999]). The controller then uses only the first control input, i.e. 
it sets u{k) = u'^{k), and reformulates the quadratic program 
at the next time step. In this paper, we use a time horizon of 
length one (A^ = 1), and recompute the A and B matrices in 
Eq. [T] at each time step. 

In the rest of this section, we describe our model predictive 
controller for manipulation with multiple contacts. First, in 



Sec. Ill- A we give an overview of the controller that we have 
developed. Next, we present the hierarchy of controllers run- 



ning on our robot in Sec. III-B In Sec. III-C we describe the 
linear quasi- static model that our model predictive controller 
uses, and detail the quadratic program that we solve at each 
time step Sec. III-D[ We then describe some extensions to the 
quadratic program in Sec. [III-E 



A. Overview of the One-Step Model Predictive Controller 

The model predictive controller that we have developed uses 
a linear discrete time model of the system, a one step time 
horizon, and attempts to move the end effector along a straight 
line to the goal subject to constraints on the predicted contact 
forces. 

It explicitly allows the robot to apply any force less than a 
don 't care force threshold at each contact. Our controller has 
the following parameters that influence its behavior: 

• Goal location {xg G 5R^).* This is the location that the 
controller attempts to move the end effector to. 

• Contact stiffness matrices {Kc- G 3?^^^).' These are the 
controller's estimates of the stiffness matrices for each 
contact location along the arm. In this paper, we assume 
that the stiffness at each contact is non-zero along the 
direction normal to the surface of the robot arm and is 
zero in the other directions. 

• Don't care force thresholds (^f thresh ^ s^sy. j^^ ^^^_ 
troller attempts to keep the force at each contact below 



Goal Location 



to command torques at the joints, r, that are given by 
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(Model Predictive Controller) 50-lOOHz 




Fig. 5: Block diagram showing the hierarchical control struc- 



ture and the equations of motion. Details are in Sec. Ill The 
controller frequencies are specific to our implementation. 



this value, and applies no penalty to contact forces below 
this threshold. 

• Maximum rate of change of contact force (A/^^^^ G 3?^).' 
This term limits the predicted change in the contact force 
over one time step with the goal of preventing large and 
abrupt changes in the contact force. 

. Safety force threshold {f^^^^'^y G 5R^).- If the contact 
force, /c-, exceeds this safety threshold value, the con- 
troller stops updating the virtual trajectory and we report 
it as a failure of the controller. 

In this paper, we perform experiments on three different 



testbeds, described in Sec. |IV] The precise meaning of contact, 
and thus the don Y care force threshold and other parameters 
of the model predictive controller, depends on the specific 
testbed, as described in Sec. |IV-E| 



Kj and Dj are constant m x m diagonal joint-space stiffness 
and damping matrices, G 3?^ and 6 G 3?"^ are the current 
joint angles and joint velocities, and fg G 3f?^ is a gravity 
compensating torque vector which is a function of 0. The robot 
arm has m joints. 

As a result, the closed loop system behaves as if the arm is 

connected to the joint- space virtual trajectory, (j), via torsional 

"Simple" Impedance visco-elastic Springs at the joints. If (j) is held constant, 

^2£^2L£2i^LE? "simple" impedance control can be shown to result in stable 

interaction with passive environments for contacts all over the 

arm ( |Hogan| |1988| |Hogan and Buerger| |2QQ5 ). 

Unlike other approaches to force control and impedance 
control, "simple" impedance control does not explicitly model 
the dynamics of the arm nor the impedance at the end effector 
dAlbu-Schaffer et al| [20031 |Sentis et al| [20T0l ). We have found 
in our previous work that in practice this form of impedance 
control, also referred to as "equilibrium point control", allows 
the robot to interact with the world in a stable, compliant, and 
effective way ( Edsinger and Kemp 2007a|b[ Jain and Kemp| 
|2009a|bl |2QTQbl K 

Other researchers have looked at similar robotic control 
strategies in simulatio n (|Gu and Ballard[ |2006| ), in free- space 
motions ( Williamson} |1996| ), in legged locomotion ( Migliore] 
'2009|), and in rhythmic manipulation from a fixed based 
( Wilfiamsonl [1999]). 

2) Model Predictive Controller: The model predictive con- 
troller is part of the outer feedback control loop that runs 
between 50-lOOHz in our implementation, as shown in Fig.|5] 
The input is a goal location, Xg G 3?^, that the controller 
attempts to reach. The controller uses feedback from the joint 
encoders and the tactile skin to compute A(/)* G 3?^, an 
incremental change in the virtual joint-space trajectory. This 
A(/)* is the control input, u{k), of Eq. [T] 

We will now derive the model predictive controller. 



B. Control Structure 

In this work, we use a hierarchical control structure with an 
inner IkHz real time joint space impedance controller, termed 



"simple" impedance control by Ho gan and Buerger ( 2005 ), 
and an outer model predictive controller that runs at 50-lOOHz, 
as shown in Fig. [5] 

Researchers have argued for the benefits of robots with low 
mechanical impedance ( [Buergerj |2006[ [Pratt| |2002| ). As has 
often been noted, these arguments are particularly relevant 
for manipulation in unstructured environments, since robots 
are likely to be uncertain about the state of the world. At 
minimum, low impedance can reduce the forces and moments 
resulting from unpredicted contact, and thus reduce the risk 
of damage to the robot, environment, and nearby people. 

1) ^^Simple'' Impedance Control: For a detailed description 
and analysis of this form of impedance control, we refer 
the reader to ' Hogan and Buerger] ([2005]). The input to the 
IkHz "simple" impedance controller, 0, is called a virtual 
trajectory. The controller uses feedback from the joint encoders 



C. Linear Discrete -Time Model 

In this section, we derive a discrete time linear quasi-static 
model, similar to Eq. [T] for the arm and its interaction with the 
world that our model predictive controller uses. Specifically, 
the model will be of the form 



0{k^l) = 0{k)^BA(l){k), 



(3) 



where G 3?^ is the state of the system (vector of joint angles 
for a robot with m joints), the control input Acj) G 3?^ is the 
incremental change in the joint- space virtual trajectory of the 
impedance controller, and B G JJ^^^. 

We begin by assuming that the robot has a fixed and 
statically stable mobile base and the arm is in contact with 
the world at n locations. We denote the i^^ contact as c^. The 
equations of motion in joint space are 



Mi9)9 + Ci9, 9)9 + Y, Jimfc. + T,{9) 



(4) 



Hand ^^''^ 




nrn 



Fig. 6: Graphical representation of a planar version of the 
quasi-static mechanical model with torsional springs at the 
joints of the robot and linear springs at contacts that our 
model predictive controller uses, as described in Sec. 



III-C 



where fc- G 3?^ is the force at the i^^ contact, Jc- G JJ^^^ is 
the Jacobian matrix for contact Ci, Tg e 3?^ is the vector of 
torques due to gravity at each joint, and r G 5R^ is the vector 
of torques appHed by the actuators at the joints. Eq. [4] ignores 
effects such as friction at the joints, but is commonly used in 
robotics ( .Featherstone and Qrin, 2008 j . 

Combining the equations of motion (Eq. |4]) with the 
impedance control law (Eq. [2]) gives us the model of the arm 
and its interaction with the world as 

n 

MO ^CO^Yl Jlf^^ ^^9= ^^(^ - ^) + ^0^ + ^9' ^^) 

In this paper, as an approximation, we assume that the dynam- 
ics are negligible, and that the gravity compensating torques 
are perfect. So, we remove all terms with 6 or 6 from Eq. |5J 

and set Tg = Tg to get 



, "'ci ici 



KM-0). 



(6) 



which is a quasi-static model. In Eq.[6j the torques at the joints 
due to the contact forces (left-hand side) balance the torques 
applied by the actuators in the joints (right-hand side). 

For the contact model, we ignore friction at the contacts 
and assume that each contact behaves like a linear spring with 
the contact force along the normal vector of the surface of 
the robot arm. These assumptions are similar to the Hertzian 
contact model ( (Johnson and Johnson| |1987t |Kao et al] |2008| ). 
This results in a mechanical model with torsional springs at 
the joints and linear springs at the contacts, shown in Fig. [6] 

If we take the difference of Eq. [6] at time instants k and 
/c + 1, we get 

n 
^ Jl{k + l)/e.(fc + 1) - Jl{k)fe,{k) = 
i=l 

Kj{^{k + 1) - ^{k) - 0{k + 1) + 0{k)). (7) 



We assume that the change in the configuration of the arm in 
one time step, 6{k -\- 1) — 0{k), is small and we approximate 
Jci{k + 1) with Jciik). This reduces Eq. |7]to 

n 

Y,JimfcAk+i)-fcAk)) = 

Kj{A^{k)-0{k^l)^0{k)), (8) 

where A(/)(/c) = (/)(/c + 1) — (/>(/c) is the control input of the 
model predictive controller, see Eq. [3] and Fig. [5] 

Using the linear elastic spring model for the contacts, shown 
in Fig. [6| 



f,^{k^l)-f,^{k) = K,^J,AO{k), 



(9) 



where AO{k) = 0{k + 1) - 0{k). We can now use Eq. [9]to 
rewrite Eq. [8] as 

(10) 

(^j + Zir=i "^J^ci-^cj is the sum of a positive definite 
matrix, Kj, and positive semi-definite matrices, JJ.Kc-Ja, 
and is therefore positive definite and invertible. 

Eq. 10 is in the same form as Eqns. [T] and [3] This is the 
linear discrete time model of the system that our controller 
generates and uses at each time step. We use contact forces 
and locations from whole-arm tactile sensing, and joint angles 
from encoders at the joints to estimate /c-, Ja, and Kc^. 



The linear form of Eq. 10 allows us to frame the opti- 
mization as a quadratic program, which can be solved effi- 
ciently (Sec. III-D| ). Additionally, we empirically demonstrate 
in Sec. VI that our controller performs well in the task of 
reaching to a goal location in cluttered environments. 



D. Quadratic Program to Compute A0* 

In this section we describe the quadratic program (QP) that 
our model predictive controller solves at each time step. 

Specifically, using the terminology of |Boyd and VandeiT 
I berghe ( 2004| ), our optimization variable is Acj), an incremental 
change in the joint- space virtual trajectory, and we minimize 
a quadratic objective function subject to linear equality and 
inequality constraints. We use the open source OpenOpt frame- 
work to solve the quadratic program ( |Kroshko[ |2011|. 

In this paper, the objective function is of the form 



^a, 



9i^ 



(11) 



where gi are quadratic functions of the optimization variable 
A0, and ai are empirically tuned scalar weights. 

We set up the quadratic program such that the solution, 
A0*, will result in the predicted position of the end effector 
which is closest to a desired position subject to constraints on 
the predicted change in the joint angles and contact forces. 



1) Move to a Desired Position: The first term of quadratic 



objective function of Eg. pT] attempts to move the end effector 
to a desired position. It is of the form 



gi = \\Axd - Axh\ 



(12) 



where Axh = Xh{k -\- 1) — Xh{k) is the predicted motion of 
the end effector (or hand) and Axd G 5R^ is the desired change 
in the end effector position in one time step. In this paper, we 
attempt to move the end effector in a straight fine towards the 
goal, Xg e ^^, and compute 



•^ g -^ h 



Axd 



if \\xg -Xh\\ > du 
if \\xg -Xh\\ < du 



(13) 



W^g-XhW 

where d^ is a small constant distance. We use the kinematic 
relationship 



Axh = JhAO, 



(14) 



where Jh G 5R^^^ is the Jacobian at the end effector (or hand), 
and AO = 6{k -\- 1) — 0{k) is the change in the joint angles 
predicted by the linear quasi-static discrete time system model 



of Eq. [To] We can now express the objective function gi as a 
quadratic function of A(/): 



gi 



Axd - A U, + E ^cT^^. ^ Kj^^ 



i=l 



(15) 



2) Joint Limits: We also add two linear inequality con- 
straints to keep the predicted joint angles within the physical 
joint limits. These are of the form 



A^, 



<A0< AOr, 



(16) 



where Admin and Admax are the difference between the min- 
imum and maximum joint limits and the current configuration 



of the robot. Using Eq. 10 we can rewrite the inequalities of 



Eq. 16 as linear inequalities in A(/). 

3) Contact Forces: For each contact, we attempt to restrict 
the contact force /c . to be below a don 't care force threshold 
jthresh ^^^ jjj^j^ ^j^^ predicted change of the contact force, 
Afc- = /c-(A: + 1) — fc-{k), in one time step. This results in 
two inequalities for each contact. 



Afmin < Afc^ < Afmax, whcrC 

Afrmn = - flf^ and 

rate fthresh 



A/, 



mm 



/ frate £th; 
\J Ci ^ J Ci 



u 



(17) 
(18) 
(19) 



jrate j^ ^ threshold on the maximum allowed predicted change 
in the contact force in one time step. The term (^f^hresh _ 
fa) in Eq. [l9| explicitly allows contact forces below /^f^^*^ 
without any additional cost. 

From Eqns. |9] and [T0| the inequalities of Eq. [T7] can be 
expressed as linear inequalities in A^. 

E. Extensions to the Quadratic Program 

In this section, we describe three extensions to the quadratic 



program of the previous section (Sec. III-D ) that we use in the 



experiments of Sec. VI 



1) Squared Magnitude of At : To discourage large changes 
in the joint torques in one time step, we add a term 

|2 



92 



IIArll 
A(f^KjKjA(^, 



(20) 



to the objective function after multiplying it with a scalar 



weight Qf2, see Eq. [TT] This term is useful in preventing large 
motions of the redundant degrees of freedom. 

2) Decrease Contact Forces Above Don 't Care Threshold: 

Due to modeling errors and unmodeled dynamics, the force at 
some contact (or a number of contacts) can go above the don 't 
care force threshold {fc^ > /cf^^*^). In this case, we modify 



the inequality constraints of Sec. |III-D3| for these contacts to 
prevent an increase in the predicted force. We also add an 
additional term gs to the objective function that encourages 
the controller to decrease the forces at these contacts. This is 
of the form 

^3 = V \\Af^^ - Afjf if fa > /,^f-^ 



(21) 



where A/^. is the desired change the contact force in one time 
step and Afc- is the change in the contact force as predicted 
by the linear model that our controller uses. We set A/^. as 
a force with a constant magnitude and a direction opposite to 
fc-. Using Eqns. [9] and 10 we can express gs as a quadratic 
function of A^. 

3) Limits on the Virtual Trajectory: On the robot Cody, 



described in Sec. |IV-B| the joint-space impedance controller 
limits the virtual trajectory to be within the physical joint 
limits. To account for this, we add two additional linear 
constraints on A(/): 



A^rnin < A^ < A^^ 



(22) 



IV. Experimental Testbeds 

We evaluated our model predictive controller using three 
different testbeds: 1) a software simulation testbed with a 3 
DoF planar arm, 2) a hardware-in-the-loop skin simulation 
testbed with a real 7 DoF arm, and 3) a skin sensor covering 
the forearm of a real 7 DoF arm. The same MPC code 
written in Python runs on all three experimental testbeds. For 
visualization, we use the rviz program which is part of the 
Robot Operating System ( [Quigley et al||2009" l. 



A. Software Simulation 

This testbed allows us to simulate a large number of trials. 
We use the open source physics simulation library. Open 
Dynamics Engine ( Smith et al 201 1| ), to simulate a planar 
arm with three rotational joints, a IkHz joint-space impedance 
controller, and tactile skin covering the entire surface of the 
arm with a simulated taxel resolution of 100 taxels per meter. 
Fig. [7] shows a visualization of the simulated robot, tactile 
skin, and taxels. 

The simulated three link planar arm has kinematics and joint 
limits similar to a human operating in a plane parallel to the 
ground at shoulder height with a fixed wrist. The three joints 
correspond to torso rotation, shoulder, and elbow. 
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Fig. 7: L^/if; Visualization of the three link planar arm 
with tactile skin interacting with obstacles within the software 
simulation testbed. Red obstacles are rigid and fixed, while 
gray obstacles are rigid and movable. Right: Visualization of 
the whole-arm tactile skin. The orange points are 1cm apart 
and represent the centers of the simulated taxels. The green 
arrows are the contact force vectors and the red arrows are 
the normal components of the contact forces. 



7 DoF MekaArms 

with SEAS at the joints 

Meka forearm tactile 
sl<in sensor 
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Fig. 8: The robot Cody with two compliant 7 DoF arms and 
a tactile skin sensor covering its right forearm. 



The arm interacts with rigid cyhndrical obstacles that are 
either fixed or movable. In isolation, a movable object can slide 
in the plane if the force applied to it exceeds friction (~ 2A^), 
while the fixed obstacles remain stationary regardless of the 
force applied to them. 

B. The Robot 

Fig. [8] shows the robot Cody that we use for experiments in 
this paper. Cody has two compliant 7 DoF arms from Meka 
Robotics with series elastic actuators (SEAs) for torque control 
at each degree of freedom. The joint space impedance control 
on Cody runs at IkHz. Cody has a S eg way omnidirectional 
mobile base which we control with a PID controller that uses 



visual odometry as described in our previous work ( [Killpack 
[eFall |2QTo1 ). 

As part of this research we have developed a tactile skin 
sensor that covers the right forearm of Cody, as shown in 
Fig. [9] and described in Sec. IV-C We currently have tactile 
skin covering its right forearm only. 

For experiments in realistic conditions, described in 
Sec. |VI-D| we wanted to be able to sense contact forces on 
more distal parts of the arm. To do this, we 3D printed a 
cylindrical cover for the wrist of the robot, shown in Fig. |9] 
We use the wrist force-torque sensor to measure the resultant 
force applied to the environment by the distal part of the arm 
beyond the forearm. Due to this cover, the experiments with 
the forearm tactile skin sensor use only the first four degrees 
of freedom of the arm. 

C. Real Tactile Skin Sensor 

Fig. |9] shows the tactile skin sensor that covers the forearm 
of the robot Cody. Meka Robotics and the Georgia Tech 
Healthcare Robotics Lab developed the forearm tactile skin 
sensor, which is based on Stanford's capacitive sensing tech- 
nology, as described in |Ulmen et al| ( [20T2] ). 

The forearm skin sensor consists of 384 taxels arranged 
in a 16 X 24 array. There are 16 taxels along the length of 
the cylindrical forearm and 24 taxels along the circumference. 




i 



Forearm skirT" ^ 



Cover for 
the wrist 




Fig. 9: Left: Tactile skin sensor on the right forearm of Cody 
(underneath the black neoprene sleeve) as well as a 3D printed 
cover for the wrist. Right: Two additional layers (thin white 
compression sleeve and black padded sleeve) that we added 
on top of the tactile skin sensor (blue) once we mounted it on 
the robot. 

Each element has a dimension of 9mm x 9mm and a sensing 
range of — 30 A/". We can obtain the 16 x 24 taxel array sensor 
data at lOOHz using Robot Operating System (ROS) drivers. 
On the robot Cody, we added two additional layers on top 
of the forearm skin sensor to cover the open parts of the joints, 
protect the skin sensor, and make the exterior of the arm low 
friction. These are shown on the right in Fig. [9] The white 
sleeve is a thin neoprene McDavid compression arm sleeve, 
and the black layer is a padded Ergodyne neoprene elbow 
sleeve designed for human athletes. 

D. Hardware -in-the -loop Skin Simulation 

Since we currently do not have whole-arm tactile skin on 
Cody, we have built a hardware-in-the-loop simulation testbed 
to be able to simulate whole-arm skin and test our controller 
on a real robot arm. 

Fig. [To] shows the current implementation of this testbed. 
We use an OptiTrak motion tracking system to register the 
positions of the obstacles, and the pose of the the robot in a 
common coordinate frame. We then use geometric collision 
detection from OpenRAVE (Diankov and Kuffner 2QQ8| ) and 
models of the robot arm and obstacles to estimate the region 
over which each link of the robot makes contact with the 
obstacles. 
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Fig. 10: Left: Different components of the hardware-in-the-loop testbed. Middle Left: Close-up of one instrumented obstacle 
showing the force-torque sensor at the base of an extruded aluminum rod which we have covered in bubble wrap. Middle 
Right: Cody attempting to reach to a goal location (green). Right: Visualization of the simulated tactile skin. 



We have also mounted a six-axis force-torque sensor at the 
base of each obstacle to measure the resultant force applied 
to each obstacle. This testbed allows us to simulate skin on a 
real robot with 7 DoF arms. 

For every instrumented obstacle and robot link pair, we 
estimate at most one contact location as the centroid of the 
contact region and use the force measured by the force-torque 
sensor as the contact force. If multiple links make contact 
with the same obstacle, we divide the force vector's magnitude 
equally among all the links. 

E. Tactile Feedback in the Different Testbeds 

There are differences in the tactile feedback in each of the 
three testbeds that change the precise meaning of contact force 
and contact location. 

Within the software simulation testbed (Sec. |IV-A| ), contact 
force refers to the normal component of the force applied by 
the robot to the environment over the surface covered by one 
taxel of the simulated tactile skin. Contact location refers to 
the centroid of the simulated taxel. 

Within the hardware-in-the-loop skin simulation testbed 
(Sec. |IV-D| ), contact force refers our estimate of the force that 
a link of the real robot applies to an object. We use at most one 
contact location between each link of the robot and an object as 
the centroid of the contact region computed using geometric 
collision detection. We currently do not simulate individual 
taxels or compute the normal component of the contact force 
within the hardware-in-the-loop skin simulation testbed. 

Lastly, on the real robot with the tactile skin sensor, for 
contacts on the forearm of the robot, contact force refers to 
the normal force applied to the environment as measured by 
one taxel of the real tactile skin sensor (Sec. |IV-C| ). Contact 
location refers to the centroid of the taxel. Additionally, for 
the distal part of the arm beyond the forearm, we get a single 
resultant contact force using a wrist force-torque sensor, as 
described in Sec. |IV-B| and we use the center of the force- 
torque sensor as the contact location. 

F. Low Stiffness at the Joints 

We use the impedance controller to maintain low stiffness at 
the joints of both the real robot Cody and the robot within the 
software simulation. Within software simulation, the robot has 



joint stiffness values of 30, 20, and 15 Nm/rad from proximal 
to distal joints. These values are similar to measured stiffnesses 



of humans during planar reaching motions ( Shadmehr 1993 ). 

On Cody, we use the same stiffness settings as our previous 
work ( |Jain and Kemp) |2009b| |2010b| ). We set the stiffness 
for the three degrees of freedom in the shoulder at 20, 50, 
and 15 Nm/rad, one DoF in the elbow at 25Nm/rad and 
2.5Nm/rad for the wrist roll degree of freedom. For the last 
two wrist joints, the robot uses position control that relates 
the motor output to joint encoder readings and ignores torque 
estimates from the deflection of the springs. Consequently, 
the wrist is held stiff, except for the passive compliance of the 
SEA springs and cables connecting the SEA to the joints. 

These stiffness settings are lower by a factor of between 400 
and 1000 than the PUMA 560 manipulator ( |Kim and Streit| 
1995). In some postures, the stiffness at the end effector is a 
factor of five lower than work on door opening with Cartesian 
impedance control described in |Ott et al| ( |2007| ). 



V. Approaches used for Comparison 

In this section we describe two approaches against which 
we compared our model predictive controller's performance. 
The first is a baseline controller, and the second is a state of 
the art geometric motion planner that has full knowledge of the 
environment. We performed comparisons against the baseline 
controller both on the real robot and in software simulation. 
We used the geometric motion planner to estimate optimal 
success rates for trials in software simulation as detailed in 
Sec.lV-B] 

A. Baseline Controller 

Our baseline controller uses the same joint- space impedance 
control as the model predictive controller to maintain low 
stiffness at the joints. However, it does not use feedback from 
the tactile skin except to define a safety stopping criterion. 
Specifically, this controller computes 

A(/)* = (j^A)"'j^Ax,, (23) 

where A^* G 5R^ is the incremental change in the joint- space 
virtual trajectory (see Fig. [5]), Jh G JJ^^^ is the Jacobian at 
the robot's end effector (or hand) and Axd G 5R^ is the desired 
Cartesian motion of the end effector computed from Eq. [13 



The baseline controller monitors the tactile skin sensor values 
and stops if the force at any contact goes above the safety 
force threshold, f^^^^^^- 



TABLE I: Results from 2420 trials in software simulation. 



If we ignore joint limits, use only gi (see Eq. [12]) as the 
objective function, and the arm is not in contact with the world, 
then Eq. [23] is the solution of the quadratic program for our 
model predictive controller. In free-space both controllers will 
attempt to move the end effector along a straight line to the 
goal, with identical low stiffness settings at the joints. 

In previous work, we have shown that a robot with low 
stiffness at the joints can successfully open doors and drawers 



with linear virtual trajectories for the end effector ( |Jain and 
Kemp] [2009a|bl ). 



B. Motion Planner 

For experiments in the software simulation testbed we 
also compare against a bi-directional RRT motion planner 
as implemented in OpenRAVE ( [Diankov and Kuffner[ |20Q8| ). 
The motion planner has complete knowledge of the cluttered 
environment and ignores movable obstacles. We remove the 
movable obstacles because the motion planner that we use 
does not plan for movable obstacles. 

We use the bi-directional RRT motion planner to estimate 
whether or not a solution exists for a given goal location 
and configuration of the clutter. We use this to estimate what 
the best success rate would be for a given set of trials. It is 
important to note that this could be an over estimate, since 
in some situations it may be impossible to remove all of the 
movable obstacles, and the remaining movable obstacles might 
block potential solutions. 



VI. Experiments 

We now describe the experiments that we performed to test 
our model predictive controller. Through these experiments, 
we empirically demonstrate that our controller can effectively 
control three different robot arms, a simulated 3 DoF of planar 
arm with simulated tactile skin (Sec. |VI-B| ), a real 7 DoF 
arm with torque controlled joints and simulated tactile skin 
(Sec. |VI-C| ), and the first 4 DoF of the same real robot arm 
with a forearm tactile skin sensor (Sec. |VI-D| ). 

We compare our controller to a motion planner (in software 
simulation) and to the baseline controller within the task of 
reaching to a goal location in a cluttered environment in Sees. 
|VI-Bl|[vrC3l and |VI-D2| 

Additionally, we provide illustrative examples of the robot 
operating in realistic conditions using the forearm tactile skin 



sensor (Sec. |VI-D1[ ), as well as ways in which the parameters 
of the model predictive controller can be used to influence its 



behavior, such as controlling the contact force (Sees. VI-B2 



and VI-C2 ), and using online estimates of contact stiffness to 



reach the goal location faster (Sec. |VI-C1[ ). 

Due to implementation differences, the precise meaning of 
contact force and contact location is different for the three 
testbeds, as described in Sec. |IV-E| 
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6 Reaches) 
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(Single Reach) 
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Controller 


Success rate 
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contact force 
Avg. contact 
force 


98.2% 


91.1% 
20. IN 

3.76N 


78.6% 

13.3N 

5.9N 


30.5% 

72.0N 

28.6N 



A. Pull Out and Retry 



In the experiments described in Sees. VI-B 1 VI-C3 and 



VI-D2 we have an additional control layer above the model 



predictive controller that makes a decision to stop the current 
controller if the end effector is not moving, pulls the arm 
out, moves the mobile base to a different location or selects a 
different starting configuration for the arm, and retries reaching 
to the goal. The details of this are outside the scope of this 
paper, and we treat this functionality as a black box for the 
current paper. 

Some of the failures in Sec. |VI-BT] were caused by a failure 
of our current approach to pulling the arm out. Pulling out 



did not fail for any of the trials described in Sees. |VI-C3] and 
IVI-D2I 



B. Software Simulation Testbed 

In this section we describe experiments on a large number 
of trials of reaching to a goal location in an environment 
consisting of fixed and movable cylindrical obstacles within 
the software simulation testbed, described in Sec. |IV-A| and 

Fig. in 

We generated multiple test trials by first deciding on the 
number of fixed and movable obstacles. We then generated 
the coordinates for the center of each cylindrical obstacle 
in succession by uniformly sampling a coordinate within a 
fixed workspace of 0.27m^. We repeated this until we found 
a collision free coordinate for each obstacle in turn. We also 
generated a random goal location, Xg, with the same sampling 
procedure. 

If accepted for publication, we will release code, data, and 
instructions to reproduce the results presented in this section. 

1) Comparison over 2420 Trials: In this experimental 
comparison, we selected 11 different values for the number 
of fixed and movable obstacles (from to 20 in steps of 2) 
and generated 20 trials for each choice of number of movable 
and fixed obstacles for a total ofllxllx20 = 2420 trials. 

We compared the estimated optimal success rate using 
the motion planner (Sec. V-B[ ) with the baseline controller 
(Sec. |V-A| ) and the model predictive controller. 

We allowed the model predictive controller to retry up to 5 
times. If an attempt to reach to the goal failed, the controller 
tried to pull the arm out to a new starting location for the end 
effector, waited for a fixed timeout period, and then retried 
reaching to the goal irrespective of the success or failure of 
pulling out. We refer to this as MPC with up to 6 reaches. 

We set the don't care force threshold, f^^^^^^^ to bN and 
the safety force threshold, f^^^^^^ to 100 N for each contact 
Cj for all the trials. 
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Fig. 11: Sequence of images showing the simulated robot successfully reaching to the goal location (green circle) using MFC 
for one of the trials within the software simulation testbed (see Sec. \yi-B2\ . The red obstacles are rigid and fixed, while the 
gray obstacles are rigid and movable. 
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Fig. 12: Median, first, and fourth quartiles for the contact 
force for 100 trials as a function of the don't care force 
threshold parameter, y^^^^s^ 



Table [T| shows the results from this comparison. The model 
predictive controller with a single reach had a success rate that 
was 48.1 percentage points more than the baseline controller, 
which corresponds to a 157.7% increase in the success rate. 
For successful trials, the average speeds of the end effector 
were comparable. The average speed was 0.049m/5 for the 
baseline controller and 0.043m/ 5 for the model predictive 
controller. 

Allowing the model predictive controller to retry further 
increased the success rate by 12.5 percentage points (a 16% 
increase). Additionally, the estimated optimal success rate 
(with full knowledge of the world and ignoring all movable 
obstacles) was 7.1 percentage points greater (or 7.8% better) 
than the model predictive controller with multiple reaches. 
Lastly, the model predictive controller kept the contact forces 
lower than the baseline controller, see Table [H 



2) Regulating Contact Forces: As described in Sec. III-A 



our model predictive controller places no penalty on contact 
forces between zero and f^^^^^^^ the don't care force thresh- 
old. 

To test the influence of /^f ^^^^ experimentally, we gener- 
ated 100 trials with 20 fixed and 20 movable obstacles. We 
then ran the model predictive controller with five different 
force thresholds on these 100 trials, and recorded all the 
contact forces every 10ms (at lOOHz). We used the same value 
for /cf ^^^^ for each contact. Fig. [ll] shows the simulated robot 
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Fig. 13: When the robot uses online estimates of the stiffness 
at the contacts (instead of conservative constants), it can push 
into deformable objects more aggressively, and complete the 



task of reaching to a goal location faster, shown in Fig. 14 



successfully reaching to the goal location for one of these 
trials. 



Fig. 12 shows the median and first and fourth quartile of 
all contact force magnitudes over the 100 trials for different 
values of /^f ^^*^. The correlation coefficient between /^f ^^^^ 
and the median force was > 0.998 providing evidence that the 
jthresh parameter of our model predictive controller can be 
used to predictably influence the contact forces. 

C. Hardware-in-the-loop Skin Simulation Testbed 

In this section we present results using a real robot and 
simulated tactile skin within the hardware-in-the-loop skin 



simulation testbed, described in Sec. |IV-D| and Fig. [TO] 

1) Online Stiffness Estimation: One of the parameters of 
our model predictive controller is the modeled stiffness at each 
of the contacts along the arm. This determines how much 
the robot is willing to move along the contact normal. For 
example, if the controller's estimate of the stiffness at a contact 
is high, it will attempt to push into that contact slowly, since 
its contact model will predict that a small motion will result 
in a large increase in the contact force. 

In this section we describe our initial efforts in estimating 
the stiffness online. We set up an experiment where the robot 
had to push into a deformable object (pillow) to reach to a 
goal location, as shown in Fig. [14] We performed two trials. 




Fig. 14: Sequence of images showing the robot pushing on a deformable red pillow to reach to a goal location (green). 



TABLE II: Model predictive controller vs the baseline con- 
troller in the hardw are -in-the -loop testbed. 





MPC 


Baseline Controller 


Success rate 

Avg. max. contact force 

Avg. contact force above 

fthresh (5N) 


5/5 

5.6N 

5.5N 


3/5 

17.7N 

14.3N 



In one trial, the robot used a static and conservative value (high 
stiffness) for the stiffness at all contact locations. In the second 
trial, the robot started with the same conservative estimates, 
but then estimated the stiffness online while interacting with 



the pillow. Fig. 13 shows that when the robot updated its 
estimate of the stiffness, it was able to push into the pillow 
more aggressively and reach the goal location faster than when 
the stiffness value was a conservative static value. 

To estimate the stiffness, we used a history of contact 
locations and contact forces as returned by the simulated tactile 
skin. We estimated the stiffness along the current contact 
normal as the slope of the line (fit using least squares) that 
describes the change in the normal component of the force 
with motion of the contact location along the contact normal. 

A video of this experiment is part of the supplementary 
materials. 

2) Selective Control of Force Applied to Different Regions 
in the Environment: With this experiment, we illustrate that 
the model predictive controller can be used to selectively 
control the contact force in different regions. We defined a 
cylindrical volume in the world as 'fragile'. If the location 
of a contact Ci in the world frame was within the 'fragile' 
volume, we set the don't care force threshold, /cf^^^^, to 2N . 
For contacts outside this volume, we set it to 5A^. f^^^^^^ is 



used in the inequality constraints of Eq. 17 



Fig. [T5] shows the forces that the robot applied to the 
environment during this trial. The histograms of contact forces 
within and outside the 'fragile' region show that the model 
predictive controller was able to selectively control the force 
that it applied to different regions in the environment. 

3) Model Predictive Controller vs Baseline Controller: 
We performed five trials with the goal location in different 
positions within the hardware-in-the-loop testbed, as shown in 



Fig. 16 In each trial, the robot moved its mobile base to up 
to three positions equally spaced along a line and facing the 




Fig. 16: Five different goal locations within the hardware-in- 
the-loop testbed that we used to compare the model predictive 



controller and the baseline controller, described in Sec. VI- C3 




Fig. 17: Image showing the sharp edge and abrasive surface 
of the cinder block used in the trial described in Sec. \VI-DI 



instrumented obstacles, and then attempted to reach to the goal 
location from a constant pre-determined arm configuration 
using the model predictive controller. The robot successfully 
reached each of the five goal locations from one of the three 
positions. We set the don't care force threshold, /^f^^*^, to 
57V and the safety force threshold /^^^""^^ to 207V for each 
contact. 

As mentioned in Sec. |VI-A| we have an additional control 
layer above the model predictive controller and the baseline 
controller. This enables the robot to exhibit behavior such as 
retrying a greedy reach from the left or right of a contact 
location. A video of these five trials with MPC and the 
additional control layer is part of the supplementary materials. 

For each goal location, we ran the baseline controller with 
the initial arm configuration and base position from which the 
model predictive controller was successful. Table |ll] shows the 
results from the five trials. The baseline controller failed in 
two out of five trials and resulted in the arm applying larger 
forces on the environment. The mean contact force (including 
the don't care interval of — bN) was 3.3N for the model 
predictive controller and S.IN for the baseline controller. 
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Fig. 15: Experiment to demonstrate selective control of contact force in different regions using the model predictive controller 
Left: Skin visualization when some contacts are within the fragile' region (red cylinder) and other contacts are outside the 
fragile ' region. The cyan circle is the goal location that the robot successfully reached. Middle: Histogram of contact forces 
within the fragile' region. Right: Histogram of contact forces outside the fragile' region. 






Fig. 18: Left: Five different goal locations that we used to compare the model predictive controller and the baseline controller, 
described in Sec. \VI-D2\ The environment consists of compliant leaves and rigid blocks of wood (outlined in red). The red 
circle denotes the position of the end effector Middle: View of the foliage from the robot's perspective. The rigid blocks of 
wood are occluded by the leaves. Right: Image of the robot after it has successfully reached goal location 5. The red circle 
denotes the end effector position. 



D. Forearm Tactile Skin Sensor 

In this section we describe results from our experiments 
with the forearm tactile skin sensor, described in Sec. |IV-C| 
and Fig. [9] Since the skin sensor currently covers only the 
forearm of the robot, we restricted our experiments to not 
have contacts on the elbow and upper arm of the robot. 

Our simulated foliage is representative of foliage found in 
nature. It consists of both compliant objects (plastic leaves) 
and rigid and fixed objects (blocks of wood). The leaves can 
result in a lot of occlusion for conventional line of sight 
sensors. Furthermore, the leaves can often be pushed aside 
with relatively low force but the blocks of wood can not. 

The cinder block is a rigid, heavy, and fixed object, rep- 
resentative of some of the objects a robot would encounter 
in rubble. The diameter of the robot's forearm (10cm) is 
close to the size of the opening of the cinder block (14.5cm). 
Additionally, the edges are sharp and the surface is abrasive 



as illustrated in Fig. 17 



1) Illustrative Examples - Foliage and Cinder Block: 

We performed one trial each of the robot reaching to a goal 
location in foliage and reaching through the opening of a 



cinder block. Fig. 20 shows two images and the histograms 
of the contact forces for these two trials. Videos of these two 
trials are part of the supplementary materials. 



TABLE III: Model predictive controller vs baseline controller 
in foliage. 





MPC 


Baseline Controller 


Success rate 

Exceeded safety threshold (15N) 
Avg. max. contact force 
Avg. contact force above 

^thresh (^5N) 


3/5 
0/20 attempts 

5.5N 

5.2N 


1/5 

19/20 attempts 

14.5N 

9.2N 



2) Model Predictive Controller vs Baseline Controller in 
Foliage: For a more careful evaluation of the model predictive 
controller and the baseline controller in realistic conditions 
using the forearm tactile skin sensor, we performed five trials 
with automatically generated goal locations that were equally 
spaced along a line within our simulated foliage, as shown in 



Fig. 18 



We started each trial by positioning the robot at the same 
location in front of the clutter. The robot then autonomously 
moved its mobile base to four roughly equally spaced positions 
along a line, and attempted to reach to the goal location 
using both the model predictive controller and the baseline 



controller, as described in Fig. 19 



Table III shows the results from a total of twenty reach 
attempts for each controller from the five trials. The model 








(a) Reaching to a goal location in foliage with multiple contacts along the arm. The forearm and 3D printed cover for the wrist are approximately outlined in red. 
The goal location is vertically below the blue bulb, and is the cyan circle in the skin visualization. 



^ 




w 


I 


Skin visualization 


Lii 


'\ . 


[J 


a 


3jiN 


, 






1^ 


1. 


k 


f 




MSI 




T k 










(b) Reaching to a goal location (green) through the opening of a cinder block. 
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(c) Histogram of contact forces while reaching to a goal location in foliage (left), and through the opening of the 
cinder block (right). 



Fig. 20: Cody reaching to a goal location in realistic conditions using its forearm tactile skin sensor, described in Sec. VI-Dl 



predictive controller successfully reached goal locations 1, 
3, and 5, while the baseline controller was only successful 
for goal location 5. Further, the model predictive controller 
successfully kept the contact forces around the don't care force 
threshold, f^^^^^^^ of 5A^. In contrast, the baseline controller 
exceeded the safety force threshold, fc^^^^^, of 15A^, 19 out 
of 20 times. The mean contact force (including the don't 
care interval of — bN) was 3.2N for the model predictive 
controller and 4.5A^ for the baseline controller. 



A video of these five trials is part of the supplementary 
materials. 



VII. Discussion 

Within this section, we discuss broader implications of our 
research, future work, and current limitations. 



A. The Big Picture 

Our results suggest that our approach is well-matched to ma- 
nipulation in real- world, high-clutter environments, although 
further evaluation is required. There are also several broader 
implications of our work, which we now discuss. 

1) Greedy Control: The performance of our system sug- 
gests that greedy feedback control can perform well in practice 
and that detailed models of the environment and long time 
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Fig. 19: Different steps that the robot performed for each of 
the five trials in the model predictive controller vs baseline 
controller comparison in foliage, described in Sec. 



VI-D2 



horizon planning may not be necessary to achieve high per- 
formance. This is similar in spirit to some research on bipedal 
walking described in Byl and Tedrake ( 2QQ8| ). 

From our perspective, if empirical research with real robots 
continues to support this conjecture, it would be a welcome 
outcome. If one considers the complexity associated with 
natural environments, such as swamps, rainforests, and caves, 
a requirement for detailed models and long time horizon 
planning seems extremely daunting, if not infeasible. Fluids, 
gasses, granular media, biological materials, and active agents 
are just a few of the complex contents found across the earth. 
That biological organisms of all shapes and sizes regularly per- 
form impressive feats of manipulation in these environments 
demonstrates that the problems are not intractable and that 
biology has found solutions worthy of emulation. Compliant 
actuation and whole-body tactile sensing combined with a 
willingness to make contact with the unknown may be impor- 
tant characteristics of the biological solution to manipulation. 

2) Reaching into the Unknown: Our results also suggest 
that reaching into the unknown can be a reasonable action for 
robots with compliant joints and whole-body tactile sensing. 
As more robots with these underlying capabilities emerge, 
the value of these attributes should become clearer, especially 
given the current rarity of whole-body tactile sensing. So far, 
we have demonstrated the feasibility of haptically reaching 
into grass-like vegetation with hidden wooden objects, into a 
constrained massive cinder block with coarse and sharp edges, 
and into a field of rigid posts covered with compliant materials. 
In these experiments, both the robot and the environment 
were unscathed in spite of repeated reaches without explicit 
foreknowledge of the environments' contents. Demonstrating 
success with more diverse environments, real natural environ- 
ments, real tactile sensing across the entire robot arm, and 
higher usable degrees of freedom will be important future 
work. 

3) Human Environments: Within this paper, we have fre- 
quently referred to natural high-clutter outdoor environments, 
such as foliage, in part because of our biological inspiration. 
However, we expect that our approach and methods would 
also be beneficial to manipulation in everyday human en- 
vironments. Humans often encounter high clutter, such as 
collections of objects on top of tables and shelves, and 



inside drawers and other containers. Humans also reach into 
constrained volumes, such as when retrieving objects from 
under furniture, cleaning hard to reach areas, or performing 
maintenance on machinery. We would expect service robots to 
benefit from comparable capabilities. Assistive robots might 
also benefit from our approach, since humans often make 
contact with their arms and other parts of their body when 
providing physical assistance to other people, such as when 
helping someone get out of bed. 

4) Emergent Intelligence: Although our low-level con- 
troller is greedy and has been provided waypoints that are 
always along a straight line from the end effector's current 
position to the goal location, to us the resulting qualitative 
motion of the robot's arm appears to be intelligent, com- 
plex, and lifelike. To objectively support these notions would 
likely require human-robot interaction studies, so they must 
be treated skeptically. Nonetheless, like Herbert Simon's ant 



walking on the beach (Simon 1996), the robot's reactions 
to the complexity of the world result in complex emergent 
motion. For example, due to tactile sensing and the controller, 
the robot can move against a compliant object until the force 
is higher than desired and then pivot around it. Maneuvers 
such as this appear to be sensible, even though they are not the 
result of explicitly planned trajectories. Likewise, the robot can 
easily respond to dynamic elements of the environment, since 
it regenerates a model at each time step based on its tactile 
sensing and greedily decides how to move. Our approach 



and results relate strongly to behavior-based robotics ( [Brooks | 
ll991j). 

B. Future Work 

The controller we have presented has promising perfor- 
mance and its properties serve to illustrate our overall ap- 
proach. Many opportunities exist to integrate this controller, 
or similar controllers, into manipulation systems. We have 
presented results with reactive behaviors, but we would expect 
the controller to also be appropriate for the execution of 
planned trajectories or commands from a teleoperator. Due to 
the greedy controller and the potential for local minima, some 
form of higher level control is required. Our approach has been 
to develop higher level controllers that detect when the arm has 
stopped making progress (reached a local minimum), and then 
restart the controller with new initial conditions. How to best 
design complementary higher level controllers and associated 
representations with memory merits further inquiry. 

There are also numerous avenues that remain open for 
further development and evaluation of this controller and 
similar controllers. For example, we have fixed the stiffness of 
the robot's joints to low constant values, which could instead 
be varied at each time step. A related open question is how to 
initialize and adapt the various controller parameters given a 
robot, an environment, and a task. Data-driven methods from 
machine learning might be a worthwhile direction for research 
related to this question. 

C. Limitations 

In spite of its good performance in our experiments, the 
current controller does have limitations that could motivate 



revisions of this controller, or new controllers entirely. First, 
our contact model consists of a linear spring, which is 
computationally favorable, but predicts adhesive forces when 
breaking contact. Second, the controller places no penalty 
on a predicted contact force that is less than f^^^^^^^ and 
has a hard inequality constraint that prevents higher predicted 
forces. Yet in practice, the actual contact forces sometimes 
exceed this constraint. Currently, the controller handles this 
by removing the constraint and adding a quadratic penalty, 
which results in an objective function that varies over time. 
It may be advantageous to instead use a constant objective 
function that is smooth, hence softening the constraint. 

Third, the current controller ignores dynamics. The resulting 
quasi-static model is well-matched to slow motions. And, slow 
motions are reasonable when performing haptically-guided 
manipulation without a model in high clutter, since a collision 
could occur at any moment. Nonetheless, taking dynamics 
into account might enable the controller to attain better per- 
formance at higher speeds, and better control of the arm's 
velocity. Fourth, so far, we have only tested the controller 
for achieving a position of the end effector. Objectives such 
as an arm posture or full pose of the end effector would be 
better matched to some tasks. These and other objectives could 
plausibly be represented as quadratic objective functions, but 
we have not tested this possibility. 

VIII. Conclusion 

We have presented our approach to manipulation, which 
from the outset emphasizes contact with the world. We as- 
sume that low contact forces are benign, and focus on the 
development of robotic systems that can control their contact 
forces during goal-directed motion. Inspired by biology, we 
assume that the robot has low-stiffness compliant actuation at 
its joints, and tactile sensing across its entire surface. 

We then described a novel controller that exploits these 
assumptions. The controller only requires haptic sensing and 
does not need a detailed model of the environment prior to 
contact. It also explicitly allows multiple contacts across the 
entire surface of the arm. 

The controller uses model predictive control (MPC) with a 
time horizon of length one, and a linear quasi-static model. 
As quantitatively summarized in the following list, we have 
empirically shown that our MPC controller enables a variety 
of robots to haptically reach goal locations in highly cluttered 
environments with low contact forces, and that it outperforms 
a baseline controller that uses the same low-stiffness actuation 
at its joints. 

1) Successfully enables a simulated robot to reach to a 
location in clutter: In an experiment with 2420 trials, 
our MPC controller succeeded in 150% more trials than 
our baseline controller, and had lower average contact 
forces (b.9N vs. 28. 6 A^). Even though it is a greedy 
controller, it was within '^8% of optimal performance 
when allowed to make up to 6 reach attempts. In 
another experiment, the correlation between the MPC 
controller's don't care force threshold and the median 
applied force was >0.998, which indicates that this 



controller parameter predictably influences the contact 
forces applied by the arm. 

2) Successfully enables a real robot with simulated 
skin to reach to a location in clutter: Using the 
MPC controller, our robot autonomously reached 5 
human specified targets, while the baseline controller 
only reached 3. The MPC controller had lower average 
maximum force for reach attempts (5.6A^ vs. 11. IN for 
baseline). In addition, we demonstrated that the robot 
can apply less force to a designated fragile region, and 
can estimate that a contact has low stiffness online 
resulting in more aggressive and efficient progress to 
a goal location. 

3) Successfully enables a real robot with real skin to 
reach to a location in real clutter: We performed a 
fully autonomous evaluation of our MPC controller in 
which it successfully commanded the real robot with 
real forearm skin to reach 3 out of 5 automatically gen- 
erated target locations within foliage (target locations 
were not necessarily achievable and could be embedded 
within rigid objects). The baseline controller succeeded 
in reaching 1 out of 5 of these targets from the same 
starting conditions as the MPC controller. The MPC 
controller also achieved lower average maximum force 
than the baseline controller (5.5A^ vs. 14. SA"), which 
corresponds to the MPC controller's don't care force 
threshold of 5A^. 

The MPC controller also enabled the robot to reach into 
a cinder block representative of rubble, which demon- 
strates the feasibility of moving against rigid, sharp and 
coarse materials, and through constrained passages. 



IX. Supplementary Materials 

1) Videos: We have prepared the following videos as part 
of the supplementary materials: 

• Model predictive controller vs baseline controller 
within the hardware-in-the-loop testbed as described in 
Sec. lvrC3] 

• Online stiffness estimation with the hardware-in-the-loop 
testbed as described in Sec. IVI-Cll 

• Illustrative examples of reaching in foliage and through 
the opening of a cinder block using the real forearm 
tactile skin sensor, described in Sec. |VI-D1| 

• Model predictive controller vs baseline controller in fo- 
liage using the forearm tactile skin sensor, as described 
in Sec. IVI-D2I 

• Video showing the "simple" impedance controller and 
low stiffness at the joints for the robot Cody. 

• The simulated robot reaching to the goal location in 
one of the trials with the software simulation testbed 
described in Sec. IVI-B2I 

2) Code: If accepted for publication, we will release our 
code as open source. We will also provide instructions and 
data to reproduce the results within the software simulation 
testbed (Sec. [vTb). 
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